#include <math.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>

#include "ColorsHSV.h"

using namespace std;

class Iplimage
{
public:
	IplImage *image;
	Iplimage(int width, int height, int depth, int chanels)
	{
		image = cvCreateImage( cvSize(width, height), depth, chanels );
	}

	Iplimage(CvCapture *capture)
	{
		image = cvQueryFrame( capture );
		if(image==NULL )
			puts("Unable to load the image!"); exit(0);
	}

	~Iplimage()
	{
		cvReleaseImage( &image );
	}

	void SetROI( CvRect rect )
	{
		cvSetImageROI( image, rect );
	}

	void ResetROI()
	{
		cvResetImageROI( image );
	}



};

int main()
{
	ColorsHSV c("boje.txt");
//	ColorsHSV c("boje.txt");
//	ColorHSV boja;
//	cout<<c.g(boja)<<endl;
//	ofstream f("boje.txt");
//	f<<c;
//	cout<<c;

	int AX=0,AY=0;
	int fspace=40, erspace=5;
	ColorHSV tl, tr;

	char taster;
	int pauza=2;
	IplImage *frame, *search;

	cvNamedWindow("original", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("search", CV_WINDOW_AUTOSIZE);

	CvCapture* capture = cvCaptureFromCAM( CV_CAP_ANY );
	if( !capture ) {fprintf( stderr, "ERROR: capture is NULL! \n" ); getchar(); exit(0);}

	search = cvCreateImage( cvSize(320, 240), 8, 3 );
	//search = cvCreateImage( cvGetSize(cvQueryFrame(capture)), 8, 3 );
	while(1)
	{
		frame = cvQueryFrame( capture ); if(frame==NULL ) { puts("Unable to load the frame!"); exit(0);}
		cvSetImageROI(frame, cvRect( 320, 240, frame->width-160, frame->height-120));
		cvResize(frame,search, CV_INTER_LINEAR);
		cvCvtColor(search, search, CV_RGB2HSV);


		AX=AY=0;
		int errorcount;
		for(int i=search->height; i>fspace; i--)
		{
			for(int j=0; j < search->width/2-fspace/2; j++)
			{
				errorcount=0;
				tr=(uchar *) &search->imageData [i*search->widthStep + (search->width/2+j)*search->nChannels];

				if(!c.r(tr) && !c.g(tr) && !c.b(tr))
				{
					for(int up=0; up<fspace; up++)
					{
						for(int s=0; s<fspace/2; s++)
						{

							tr= (uchar *)&search->imageData [(i-up)*search->widthStep + (search->width/2+j+s) * search->nChannels];
							if(c.r(tr) || c.g(tr) || c.b(tr))
								errorcount++;

							tr= (uchar *)&search->imageData [(i-up)*search->widthStep + (search->width/2+j-s) * search->nChannels];
							if(c.r(tr) || c.g(tr) || c.b(tr))
								errorcount++;

							if(errorcount>=erspace)
								break;
						}
						if(errorcount>=erspace)
							break;

					}
					if(errorcount<erspace)
					{
						AX=j;
						AY=search->height-i;
						break;
					}
				}

				if(AY)
					break;

				tl=(uchar *) &search->imageData [i*search->widthStep - (search->width/2+j)*search->nChannels];

				if(!c.r(tl) && !c.g(tl) && !c.b(tl))
				{

					for(int up=0; up<fspace; up++)
					{
						for(int s=0; s<fspace/2; s++)
						{

							tl= (uchar *)&search->imageData [(i-up)*search->widthStep - (search->width/2+j+s) * search->nChannels];

							if(c.r(tl) || c.g(tl) || c.b(tl))
								errorcount++;

							tl= (uchar *)&search->imageData [(i-up)*search->widthStep - (search->width/2+j-s) * search->nChannels];

							if(c.r(tl) || c.g(tl) || c.b(tl))
								errorcount++;

							if(errorcount>=erspace)
								break;
						}
						if(errorcount>=erspace)
							break;
					}

					if(errorcount<erspace)
					{
						AX=-j;
						AY=search->height-i;
						break;
					}
				}

			}
			if(AY)
			{
				cout<<"Error count: "<<errorcount<<" ";
//				AY=search->height-i;
				break;
			}

		}

		for(int i=0; i<(search->height*search->width*search->nChannels); i+=search->nChannels)
		{
			if(c.r( (uchar *) &search->imageData[i]))
			{
				search->imageData[i]=0;
				search->imageData[i+1]=0;
				search->imageData[i+2]=254;
			}
			else if(c.g( (uchar *) &search->imageData[i]))
			{
				search->imageData[i]=0;
				search->imageData[i+1]=254;
				search->imageData[i+2]=0;
			}
			else if(c.b( (uchar *) &search->imageData[i]))
			{
				search->imageData[i]=254;
				search->imageData[i+1]=0;
				search->imageData[i+2]=0;
			}
			else if(c.d( (uchar *) &search->imageData[i]))
			{
				search->imageData[i]=0;
				search->imageData[i+1]=254;
				search->imageData[i+2]=254;
			}
			else
			{
				search->imageData[i]=0;
				search->imageData[i+1]=0;
				search->imageData[i+2]=0;
			}

		}
		cvShowImage("original",frame);
		cvShowImage("search",search);

		cout<<"AX: "<<AX<<"  AY: "<<AY<<endl;

		taster=cvWaitKey(pauza);
//		switch(taster)
//		{
//			break;
//		}
		if(taster==27) break;
	}
	cvDestroyWindow("original");
	cvDestroyWindow("search");
	return 0;
}
